
#define BOOST_TEST_MODULE MMM_LEGACY_CUSTOM_XML_TEST

#include <MMM/Model/Model.h>
#include <MMM/Motion/Legacy/LegacyMotion.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <MMM/RapidXML/rapidxml.hpp>
#include <string>

#include <boost/test/unit_test.hpp>


//if BOOST 1.53 or higher
#include <boost/smart_ptr.hpp>


#include <Eigen/Core>
#include <Eigen/Geometry>

// BOOST_AUTO_TEST_SUITE(XMLCustomParsingTest)


class CustomData : public MMM::MotionEntry
{
public:
	CustomData():MotionEntry("MyCustomRootTag")
	{
		s = "not set";
	}

	//! Generate XML tag
	std::string toXML() override
	{
		std::string tab = "\t\t\t\t";
		std::stringstream res;
		res << "<" << tagName << ">"<< std::endl;
		res << tab << "<SecondLevelTag>" << s << "</SecondLevelTag>" << std::endl;
		res << "</" << tagName << ">"<< std::endl;
		return res.str();
	}

	std::string s;
};
using CustomDataPtr = boost::shared_ptr<CustomData>;

class CustomRootTagReader : public MMM::XMLMotionTagProcessor
{
public:
	CustomRootTagReader() :XMLMotionTagProcessor(){};
	bool processMotionXMLTag(rapidxml::xml_node<char>* tag, MMM::LegacyMotionPtr motion ) override
	{
		if (!tag)
			return false;
		std::string namestr = tag->name();
		if (namestr!="MyCustomRootTag")
			return false;
		rapidxml::xml_node<>* node = tag->first_node("secondleveltag",0,false);
		if (!node)
			return false;
		std::string v = node->value();
		CustomDataPtr cmd(new CustomData());
		cmd->s = v;
		return motion->addEntry("MyCustomRootTag",cmd);
	}
};
using CustomRootTagReaderPtr = boost::shared_ptr<CustomRootTagReader>;

BOOST_AUTO_TEST_CASE(testParseLoadMotionCustomRootTags)
{
	const std::string motionString = 
		"<?xml version='1.0' encoding='UTF-8'?>"
		"<mmm>"
		"<motion>"
		"<comments>"
		"   <text>Line1</text>"
		"   <text>Line2</text>"
		"</comments>"
		"<jointorder>"
		"    <joint name='j1'/>"
		"    <joint name='j2'/>"
		"</jointorder>"
		"<MyCustomRootTag>"
		"	<SecondLevelTag>ABC</SecondLevelTag>"
		"</MyCustomRootTag>"
		"   <motionframes>"
		"   <motionframe>"
		"       <timestep>0</timestep>"
		"       <RootPosition>1 2 3</RootPosition>"
		"       <RootPositionVelocity>4 5 6</RootPositionVelocity>"
		"       <RootPositionAcceleration>7 8 9</RootPositionAcceleration>"
		"       <RootRotation>10 11 12</RootRotation>"
		"       <RootRotationVelocity>13 14 15</RootRotationVelocity>"
		"       <RootRotationAcceleration>16 17 18</RootRotationAcceleration>"
		"       <jointPosition>20 21</jointPosition>"
		"       <jointVelocity>22 23</jointVelocity>"
		"       <jointAcceleration>24 25</jointAcceleration>"
		"   </motionframe>"
		"   </motionframes>"
		"</motion>"
		"</mmm>";

    MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());
	MMM::LegacyMotionPtr m = r->createMotionFromString(motionString);
	BOOST_REQUIRE(m);
	BOOST_REQUIRE(!m->getComment().empty());
	BOOST_REQUIRE(m->getNumFrames()==1);
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootPos().isApprox(Eigen::Vector3f(1.0f,2.0f,3.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootPosVel().isApprox(Eigen::Vector3f(4.0f,5.0f,6.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootPosAcc().isApprox(Eigen::Vector3f(7.0f,8.0f,9.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootRot().isApprox(Eigen::Vector3f(10.0f,11.0f,12.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootRotVel().isApprox(Eigen::Vector3f(13.0f,14.0f,15.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootRotAcc().isApprox(Eigen::Vector3f(16.0f,17.0f,18.0f)));
	BOOST_REQUIRE(m->getJointNames().size() == 2);
	BOOST_REQUIRE(m->getJointNames()[0] == "j1");
	BOOST_REQUIRE(m->getJointNames()[1] == "j2");

	// now create custom tag reader
	CustomRootTagReaderPtr customTagProcessor(new CustomRootTagReader());

	r->registerMotionXMLTag("MyCustomRootTag",customTagProcessor);
	MMM::LegacyMotionPtr m2 = r->createMotionFromString(motionString);

	BOOST_REQUIRE(m2);
	BOOST_REQUIRE(m2->getNumFrames()==1);
	BOOST_REQUIRE(!m2->getComment().empty());
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootPos().isApprox(Eigen::Vector3f(1.0f,2.0f,3.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootPosVel().isApprox(Eigen::Vector3f(4.0f,5.0f,6.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootPosAcc().isApprox(Eigen::Vector3f(7.0f,8.0f,9.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootRot().isApprox(Eigen::Vector3f(10.0f,11.0f,12.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootRotVel().isApprox(Eigen::Vector3f(13.0f,14.0f,15.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootRotAcc().isApprox(Eigen::Vector3f(16.0f,17.0f,18.0f)));
	BOOST_REQUIRE(m2->getJointNames().size() == 2);
	BOOST_REQUIRE(m2->getJointNames()[0] == "j1");
	BOOST_REQUIRE(m2->getJointNames()[1] == "j2");
	BOOST_REQUIRE(m2->hasEntry("MyCustomRootTag"));
	MMM::MotionEntryPtr md = m2->getEntry("MyCustomRootTag");
	BOOST_REQUIRE(md);
        CustomDataPtr cmd = boost::dynamic_pointer_cast<CustomData>(md);
	BOOST_REQUIRE(cmd);
	BOOST_REQUIRE(cmd->s == "ABC");


	// check export and re-load data
	std::string motionString3 = m2->toXML();
	//std::cout << motionString3 << std::endl;
	std::string start = "<MMM>";
	std::string end = "</MMM>";
	motionString3 = start + motionString3;
	motionString3 = motionString3 + end;
	MMM::LegacyMotionPtr m3 = r->createMotionFromString(motionString3);

	BOOST_REQUIRE(m3);
	BOOST_REQUIRE(m3->getNumFrames()==1);
	BOOST_REQUIRE(!m3->getComment().empty());
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootPos().isApprox(Eigen::Vector3f(1.0f,2.0f,3.0f)));
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootPosVel().isApprox(Eigen::Vector3f(4.0f,5.0f,6.0f)));
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootPosAcc().isApprox(Eigen::Vector3f(7.0f,8.0f,9.0f)));
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootRot().isApprox(Eigen::Vector3f(10.0f,11.0f,12.0f)));
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootRotVel().isApprox(Eigen::Vector3f(13.0f,14.0f,15.0f)));
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootRotAcc().isApprox(Eigen::Vector3f(16.0f,17.0f,18.0f)));
	BOOST_REQUIRE(m3->getJointNames().size() == 2);
	BOOST_REQUIRE(m3->getJointNames()[0] == "j1");
	BOOST_REQUIRE(m3->getJointNames()[1] == "j2");
	BOOST_REQUIRE(m3->hasEntry("MyCustomRootTag"));
	MMM::MotionEntryPtr md3 = m3->getEntry("MyCustomRootTag");
	BOOST_REQUIRE(md3);
        CustomDataPtr cmd3 = boost::dynamic_pointer_cast<CustomData>(md3);
	BOOST_REQUIRE(cmd3);
	BOOST_REQUIRE(cmd3->s == "ABC");

}


class CustomMotionFrame : public MMM::MotionFrameEntry
{
public:
	CustomMotionFrame() : MotionFrameEntry("MyCustomTag")
	{
		s = "not set";
	}

	//! Generate XML tag
	std::string toXML() override
	{
		std::string tab = "\t\t";
		std::stringstream res;
		res << tab << tab << "<" << tagName << ">"<< std::endl;
		res << tab << tab << tab << "<inside valid='true'>" << s << "</inside>" << std::endl;
		res << tab << tab << "</" << tagName << ">"<< std::endl;
		return res.str();
	}

	std::string s;
};
using CustomMotionFramePtr = boost::shared_ptr<CustomMotionFrame>;

class CustomTagReader : public MMM::XMLMotionFrameTagProcessor
{
public:
	CustomTagReader():XMLMotionFrameTagProcessor(){};
	bool processMotionFrameXMLTag(rapidxml::xml_node<char>* tag, MMM::MotionFramePtr motionframe ) override
	{
		if (!tag)
			return false;
		std::string namestr = tag->name();
		if (namestr!="MyCustomTag")
			return false;
		rapidxml::xml_node<>* node = tag->first_node("inside",0,false);
		if (!node)
			return false;
		rapidxml::xml_attribute<>* attr = node->first_attribute("valid",0,false);
		if (!attr)
			return false;
		std::string attrValue = attr->value();
		if (attrValue != "true")
			return false;
		std::string v = node->value();
		CustomMotionFramePtr cmd(new CustomMotionFrame());
		cmd->s = v;
		return motionframe->addEntry("MyCustomTag",cmd);
	}
};
using CustomTagReaderPtr = boost::shared_ptr<CustomTagReader>;


BOOST_AUTO_TEST_CASE(testParseLoadMotionCustomDataTags)
{
	const std::string motionString = 
		"<?xml version='1.0' encoding='UTF-8'?>"
		"<mmm>"
		"<motion>"
		"<comments>"
		"<text>Line1</text>"
		"<text>Line2</text>"
		"</comments>"
		"<jointorder>"
		"    <joint name='j1'/>"
		"    <joint name='j2'/>"
		"</jointorder>"
		"<motionframes>"
		"	<motionframe>"
		"		<timestep>0</timestep>"
		"		<MyCustomTag>"
		"			<inside valid='true'>ABC</inside>"
		"		</MyCustomTag>"
		"		<RootPosition>1 2 3</RootPosition>"
		"		<RootPositionVelocity>4 5 6</RootPositionVelocity>"
		"		<RootPositionAcceleration>7 8 9</RootPositionAcceleration>"
		"		<RootRotation>10 11 12</RootRotation>"
		"		<RootRotationVelocity>13 14 15</RootRotationVelocity>"
		"		<RootRotationAcceleration>16 17 18</RootRotationAcceleration>"
		"		<jointposition>20 21</jointposition>"
		"		<jointvelocity>22 23</jointvelocity>"
		"		<jointacceleration>24 25</jointacceleration>"
		"	</motionframe>"
		"</motionframes>"
		"</motion>"
		"</mmm>";

    MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());
	MMM::LegacyMotionPtr m = r->createMotionFromString(motionString);
	BOOST_REQUIRE(m);
	BOOST_REQUIRE(!m->getComment().empty());
	BOOST_REQUIRE(m->getNumFrames()==1);
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootPos().isApprox(Eigen::Vector3f(1.0f,2.0f,3.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootPosVel().isApprox(Eigen::Vector3f(4.0f,5.0f,6.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootPosAcc().isApprox(Eigen::Vector3f(7.0f,8.0f,9.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootRot().isApprox(Eigen::Vector3f(10.0f,11.0f,12.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootRotVel().isApprox(Eigen::Vector3f(13.0f,14.0f,15.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootRotAcc().isApprox(Eigen::Vector3f(16.0f,17.0f,18.0f)));
	BOOST_REQUIRE(m->getJointNames().size() == 2);
	BOOST_REQUIRE(m->getJointNames()[0] == "j1");
	BOOST_REQUIRE(m->getJointNames()[1] == "j2");

	// now create custom data and tag reader
	CustomTagReaderPtr customTagProcessor(new CustomTagReader());

	r->registerMotionFrameXMLTag("MyCustomTag",customTagProcessor);
	MMM::LegacyMotionPtr m2 = r->createMotionFromString(motionString);

	BOOST_REQUIRE(m2);
	BOOST_REQUIRE(m2->getNumFrames()==1);
	BOOST_REQUIRE(!m2->getComment().empty());
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootPos().isApprox(Eigen::Vector3f(1.0f,2.0f,3.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootPosVel().isApprox(Eigen::Vector3f(4.0f,5.0f,6.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootPosAcc().isApprox(Eigen::Vector3f(7.0f,8.0f,9.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootRot().isApprox(Eigen::Vector3f(10.0f,11.0f,12.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootRotVel().isApprox(Eigen::Vector3f(13.0f,14.0f,15.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootRotAcc().isApprox(Eigen::Vector3f(16.0f,17.0f,18.0f)));
	BOOST_REQUIRE(m2->getJointNames().size() == 2);
	BOOST_REQUIRE(m2->getJointNames()[0] == "j1");
	BOOST_REQUIRE(m2->getJointNames()[1] == "j2");
	BOOST_REQUIRE(m2->getMotionFrame(0)->hasEntry("MyCustomTag"));
	MMM::MotionFrameEntryPtr md = m2->getMotionFrame(0)->getEntry("MyCustomTag");
	BOOST_REQUIRE(md);
        CustomMotionFramePtr cmd = boost::dynamic_pointer_cast<CustomMotionFrame>(md);
	BOOST_REQUIRE(cmd);
	BOOST_REQUIRE(cmd->s == "ABC");


	// check export and re-load data
	std::string motionString3 = m2->toXML();
	//std::cout << motionString3 << std::endl;
	std::string start = "<MMM>";
	std::string end = "</MMM>";
	motionString3 = start + motionString3;
	motionString3 = motionString3 + end;
	MMM::LegacyMotionPtr m3 = r->createMotionFromString(motionString3);

	BOOST_REQUIRE(m3);
	BOOST_REQUIRE(m3->getNumFrames()==1);
	BOOST_REQUIRE(!m3->getComment().empty());
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootPos().isApprox(Eigen::Vector3f(1.0f,2.0f,3.0f)));
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootPosVel().isApprox(Eigen::Vector3f(4.0f,5.0f,6.0f)));
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootPosAcc().isApprox(Eigen::Vector3f(7.0f,8.0f,9.0f)));
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootRot().isApprox(Eigen::Vector3f(10.0f,11.0f,12.0f)));
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootRotVel().isApprox(Eigen::Vector3f(13.0f,14.0f,15.0f)));
	BOOST_REQUIRE(m3->getMotionFrame(0)->getRootRotAcc().isApprox(Eigen::Vector3f(16.0f,17.0f,18.0f)));
	BOOST_REQUIRE(m3->getJointNames().size() == 2);
	BOOST_REQUIRE(m3->getJointNames()[0] == "j1");
	BOOST_REQUIRE(m3->getJointNames()[1] == "j2");
	BOOST_REQUIRE(m3->getMotionFrame(0)->hasEntry("MyCustomTag"));
	MMM::MotionFrameEntryPtr md3 = m3->getMotionFrame(0)->getEntry("MyCustomTag");
	BOOST_REQUIRE(md3);
        CustomMotionFramePtr cmd3 = boost::dynamic_pointer_cast<CustomMotionFrame>(md3);
	BOOST_REQUIRE(cmd3);
	BOOST_REQUIRE(cmd3->s == "ABC");
}




// BOOST_AUTO_TEST_SUITE_END()
